#ifndef PUDGE_MATH_MAT_HPP
#define PUDGE_MATH_MAT_HPP

#include <pudge/math/vec.hpp>

namespace pudge { namespace math { 

	template<typename Real = float> struct perspective
	{
		Real	fov
			,	aspect
			,	zNear
			,	zFar;

		perspective()
			:	fov (3.14f * .25f)
			,	aspect (1.0f)
			,	zNear (1.0f)
			,	zFar (500.0f) {}
	};

	/// mat implements a 4x4 row major matrix

	template<class Real = float> struct mat 
	{
		Real 
			m11, m12, m13, m14,
			m21, m22, m23, m24,
			m31, m32, m33, m34,
			tx,  ty,  tz,  m44;

		mat() 
			:	m11(1.0), m12(0.0), m13(0.0), m14(0.0)
			,	m21(0.0), m22(1.0), m23(0.0), m24(0.0)
			,	m31(0.0), m32(0.0), m33(1.0), m34(0.0)
			,	tx(0.0),  ty(0.0),  tz(0.0), m44(1.0) {}

		Real & operator[](const int i)
		{
			return (&m11)[i];
		}
		template<int Size, class Real> 
		mat<Real>& set_translation(vec<Size,Real> const& v)
		{
			tx = v.x; ty = v.y; tz = v.z;
			return *this;
		}
		mat<Real> & set_translation(const Real x, const Real y, const Real z)
		{
			tx = x; ty = y; tz = z;
			return *this;
		}
		mat<Real>& identity()
		{
			m11 = 1.0f; m12 = 0.0f; m13 = 0.0f; m14 = 0.0f;
			m21 = 0.0f; m22 = 1.0f; m23 = 0.0f; m24 = 0.0f;
			m31 = 0.0f; m32 = 0.0f; m33 = 1.0f; m34 = 0.0f;
			tx  = 0.0f; ty  = 0.0f; tz  = 0.0f; m44 = 1.0f;
			return *this;
		}
		bool mat<Real>::operator == (mat<Real> const& m) const
		{
			for(uint i = 0; i < 16; i++)
				if((*this)[i] != m[i]) return false;
			return true;
		}
		mat<Real> & operator = (mat<Real> const& m)
		{
			m11 = m.m11; m12 = m.m12; m13 = m.m13; m14 = m.m14;
			m21 = m.m21; m22 = m.m22; m23 = m.m23; m24 = m.m24;
			m31 = m.m31; m32 = m.m32; m33 = m.m33; m34 = m.m34;
			tx = m.tx;	 ty = m.ty;   tz = m.tz;   m44 = m.m44;

			return *this;
		}

		//mat<Real> & operator = (quat<Real> const& q)
		//{
		//	const Real w = q.w;
		//	const Real x = q.x;
		//	const Real y = q.y;
		//	const Real z = q.z;

		//	m11 = 2.0f*w*w + 2.0f*x*x - 1.0f;
		//	m21 = 2.0f*x*y - 2.0f*w*z;
		//	m31 = 2.0f*x*z + 2.0f*w*y;

		//	m12 = 2.0f*x*y + 2.0f*w*z;
		//	m22 = 2.0f*w*w + 2.0f*y*y - 1.0f;
		//	m32 = 2.0f*y*z - 2.0f*w*x;

		//	m13 = 2.0f*x*z - 2.0f*w*y;
		//	m23 = 2.0f*y*z + 2.0f*w*x;
		//	m33 = 2.0f*w*w + 2.0f*z*z - 1.0f;

		//	m14 = m24 = m34 = tx = ty = tz = 0.0f;
		//	m44 = 1.0f;

		//	return *this;
		//}

		mat<Real> & operator = (perspective<Real> const& p)
		{
			Real f	= 1.0f / tan(p.fov * 0.5f);
			Real d	= p.zNear - p.zFar;

			m11 = f / p.aspect;
			m22 = f;
			m33 = (p.zFar + p.zNear) / d;	
			m34 = -1.0f;
			tz = (2 * p.zFar * p.zNear) / d;

			m12 = 0.0f;	m13 = 0.0f;	m14 = 0.0f;
			m21 = 0.0f;				m23 = 0.0f; m24 = 0.0f;
			m31 = 0.0f; m32 = 0.0f;
			tx = 0.0f; ty = 0.0f;				m44 = 0.0f;

			return *this;
		}

		mat<Real> & transpose()
		{
			Real tmp = 0.0;

			tmp = m12; m12 = m21; m21 = tmp;
			tmp = m13; m13 = m31; m31 = tmp;
			tmp = m23; m23 = m32; m32 = tmp;

			return *this;
		}

		///	This function assumes an orthonormalized matrix.
		///
		///	let A = RT and RTx = b
		///	x = inv(RT) * b = -T * trans(R) * b

		mat<Real> & fast_inv()
		{
			Real tmp = 0.0;

			tmp = m12; m12 = m21; m21 = tmp;
			tmp = m13; m13 = m31; m31 = tmp;
			tmp = m23; m23 = m32; m32 = tmp;

			const Real rx = -tx;
			const Real ry = -ty;
			const Real rz = -tz;

			tx = m11*rx + m21*ry + m31*rz;
			ty = m12*rx + m22*ry + m32*rz;
			tz = m13*rx + m23*ry + m33*rz;

			return *this;
		}

		///	Calculate determinant with the cofactor formula.

		Real det() const
		{
			return
				m11 * (m22*m33 - m23*m32) +
				m12 * (m23*m31 - m21*m33) +
				m13 * (m21*m32 - m22*m31);
		}
	};

	///	Invert a matrix by transposing the cofactor matrix and dividing by the determinant.

	template<class Real> mat<Real> & inv(mat<Real> & d, mat<Real> const& r)
	{
		Real deter = r.det();

		// avoid division by a very small number
		if(deter < 0.000001f && deter > -0.000001f)
			throw -1;

		const Real invOfDet = 1.0f / deter;

		d.m11 = (r.m22*r.m33 - r.m23*r.m32) * invOfDet;
		d.m12 = (r.m13*r.m32 - r.m12*r.m33) * invOfDet;
		d.m13 = (r.m12*r.m23 - r.m13*r.m22) * invOfDet;

		d.m21 = (r.m23*r.m31 - r.m21*r.m33) * invOfDet;
		d.m22 = (r.m11*r.m33 - r.m13*r.m31) * invOfDet;
		d.m23 = (r.m13*r.m21 - r.m11*r.m23) * invOfDet;

		d.m31 = (r.m21*r.m32 - r.m22*r.m31) * invOfDet;
		d.m32 = (r.m12*r.m31 - r.m11*r.m32) * invOfDet;
		d.m33 = (r.m11*r.m22 - r.m12*r.m21) * invOfDet;

		d.tx = -(d.m11*r.tx + d.m21*r.ty + d.m31*r.tz);
		d.ty = -(d.m12*r.tx + d.m22*r.ty + d.m32*r.tz);
		d.tz = -(d.m13*r.tx + d.m23*r.ty + d.m33*r.tz);

		return d;
	}

	/// Multiply columns of A times rows of B, AB = D 

	template<class Real> mat<Real> & mul
		(	mat<Real>& d
		,	mat<Real> const& a
		,	mat<Real> const& b )
	{
#ifdef _DEBUG
		if(&d == &a || &d == &b)
			throw -1;
#endif

		d.m11 = a.m11*b.m11 + a.m21*b.m12 + a.m31*b.m13;
		d.m12 = a.m11*b.m21 + a.m21*b.m22 + a.m31*b.m23;
		d.m13 = a.m11*b.m31 + a.m21*b.m32 + a.m31*b.m33;

		d.m11 = a.m12*b.m11 + a.m22*b.m12 + a.m32*b.m13;
		d.m12 = a.m12*b.m21 + a.m22*b.m22 + a.m32*b.m23;
		d.m13 = a.m12*b.m31 + a.m22*b.m32 + a.m32*b.m33;

		d.m11 = a.m13*b.m11 + a.m23*b.m12 + a.m33*b.m13;
		d.m12 = a.m13*b.m21 + a.m23*b.m22 + a.m33*b.m23;
		d.m13 = a.m13*b.m31 + a.m23*b.m32 + a.m33*b.m33;

		// NOTE: d.tx d.ty d.tz come out in A's row space

		d.tx = a.m11*b.tx + a.m21*b.ty + a.m31*b.tz + a.tx;
		d.ty = a.m12*b.tx + a.m22*b.ty + a.m32*b.tz + a.ty;
		d.tz = a.m13*b.tx + a.m23*b.ty + a.m33*b.tz + a.tz;

		return d;
	}

	/// Multiply columns of A times the row vector v and translate 
	/// Av + T = d

	template<int Size, class Real> vec<Size,Real>& mul
		(	vec<Size,Real> & d
		,	mat<Real> const& a
		,	vec<Size,Real> const& v)
	{
		const Real x = v.x;
		const Real y = v.y;
		const Real z = v.z;

		d.x = a.m11*x + a.m21*y + a.m31*z + a.tx;
		d.y = a.m12*x + a.m22*y + a.m32*z + a.ty;
		d.z = a.m13*x + a.m23*y + a.m33*z + a.tz;

		return d;
	}

	/// Av = d

	template<int Size, class Real> vec<Size,Real> & mul3
		(	vec<Size,Real>& d
		,	mat<Real> const& a
		,	vec<Size,Real> const& b )
	{
		const Real x = b.x;
		const Real y = b.y;
		const Real z = b.z;

		d.x = a.m11*x + a.m21*y + a.m31*z;
		d.y = a.m12*x + a.m22*y + a.m32*z;
		d.z = a.m13*x + a.m23*y + a.m33*z;

		return d;
	}

	// C++ version of gluLookAt function

	template<int Size, class Real>
	mat<Real> & look_at
		(	mat<Real> & m
		,	vec<Size,Real> const& eye	
		,	vec<Size,Real> const& pos
		,	vec<Size,Real> const& up )
	{
		vec<Size,Real> s;
		vec<Size,Real> u;
		vec<Size,Real> f;
		vec<Size,Real> e(eye.x, eye.y, eye.z);

		sub(f, pos, e);
		normalize(f);

		cross(s, f, up);
		cross(u, s, f);

		negate(f);
		negate(e);

		m.m11 = s.x; m.m12 = u.x; m.m13 = f.x; m.m14 = 0.0f;
		m.m21 = s.y; m.m22 = u.y; m.m23 = f.y; m.m24 = 0.0f;
		m.m31 = s.z; m.m32 = u.z; m.m33 = f.z; m.m34 = 0.0f;
		m.m44 = 1.0f;
		m.set_translation(mul3(e, m, e));
		return m;
	}

	
	//	Look at a target on a horizontal radius.
	//	
	//	/arg d = destination modelview matrix
	//	/arg o = origin
	//	/arg angle = angle around circle
	//	/arg distance = distance from origin
	//	/arg elevation = elevation along z axis
	
	template<class Real>
	void look_at_hori_radius
		(	mat<Real> & d
		,	vec<3,Real> const& o
		,	const Real angle
		,	const Real distance
		,	const Real elevation )
	{
		vec<3,Real> up (0.0f, 1.0f, 0.0f);
		vec<3,Real> eye
			(	cos(angle) * distance + cen.x
			,	cen.y + elevation
			,	sin(angle) * distance );

		look_at(d, eye, o, up);
	}
}}

#endif